/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "vehiclekinematics.h"
#include "math.h"
#include <QDebug>

//#define D_MAX_ROTATION_CONTROL_ANGLE 50
#define D_MAX_ROTATION_CONTROL_ANGLE 10
VehicleKinematics::VehicleKinematics(E_VEHICLE_TYPE typeIn, double wheel_diameter, double base_width,
                                     double base_heighth, double steeringKpIn, double wheelMaxSpeedIn,double maxSteerAdjustIn)
{
    type=typeIn;
    resetKinematicPara(wheel_diameter, base_width,  base_heighth);
    steeringKp=steeringKpIn;

    feedforward_x=0;
    feedforward_y=0;
    feedforward_r=0;
    debugFlag=0;
//    maxSteerAdjust=M_PI*D_MAX_ROTATION_CONTROL_ANGLE/180.0;
    maxSteerAdjust=M_PI*maxSteerAdjustIn/180.0;;

    wheelMaxSpeed=wheelMaxSpeedIn;

}

int VehicleKinematics::resetKinematicPara(double wheel_diameter, double base_width, double base_heighth)
{
    wheel_diameter_in_m= wheel_diameter;
    circumference_in_m= M_PI * wheel_diameter_in_m;
    base_width_in_m= base_width;
    base_heighth_in_m=base_heighth;
    return 1;
}

int VehicleKinematics::setSteeringControlKp(double kpIn,double maxSteerAdjustIn)
{
    qDebug()<<"setSteeringControlKp old="<<steeringKp<<" new="<<kpIn<<" maxSteerAdjustIn="<<maxSteerAdjustIn;
    steeringKp=kpIn;
    maxSteerAdjust=M_PI*maxSteerAdjustIn/180.0;

    return 1;
}

int VehicleKinematics::setDebugFlag(int flagIn)
{
    debugFlag=flagIn;
    return 1;
}

int VehicleKinematics::setVirtualLineTrack_feedfowardOutput(double feedforward_xIn, double feedforward_yIn, double feedforward_rIn)
{
    feedforward_x=feedforward_xIn;
    feedforward_y=feedforward_yIn;
    feedforward_r=feedforward_rIn;
    return 1;
}

int VehicleKinematics::setSteeringPositionFeedback(QVector<double> steeringPositionIn)
{
    for(int i=0;i<steeringPositionIn.size();i++ )
    {
        steeringPositionFeedback[i]= steeringPositionIn[i];
    }
    return 1;
}

int VehicleKinematics::transWheelVel2VehicleVel(QVector<double> wheelVel, QVector<double> steeringAngle,
                                                VehicleVelocity &velOut)
{
    switch(type)
    {
        case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
        {
            if(wheelVel.size()<2)
            {
                qDebug()<<"error,,,wheelVel.size()<2";
                return -1;
            }
            transWheelVel2VehicleVel_two_differential(wheelVel[0],wheelVel[1],velOut);
            break;
        }
    case E_VEHICLE_TYPE_MCCRUM_4_WHEEL:
    {
        if(wheelVel.size()<4)
        {
            return -1;
        }
        transWheelVel2VehicleVel_four_mecanum(wheelVel,velOut);
        break;
    }
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {
        if(wheelVel.size()<1 || steeringAngle.size()<1)
        {
            return -1;
        }
        transWheelVel2VehicleVel_oneSteering(wheelVel[0],steeringAngle[0],velOut);
        break;
    }
        default:
        {
        return -1;
        }
    }
    return 1;
}

int VehicleKinematics::transVehicleVel2WheelVel(E_VEHICLE_MODE modeIn,VehicleVelocity vehicleVel,
                                                QVector<double> &steeringAngleOut,
                                                QVector<double>& wheelVel)
{
    switch(type)
    {
        case E_VEHICLE_TYPE_TWO_DIFFERENTIAL_DRIVE:
        {
            transVehicleVel2WheelVel_two_differential(vehicleVel.x_move,vehicleVel.z_rotate,wheelVel);
            break;
        }
    case E_VEHICLE_TYPE_MCCRUM_4_WHEEL:
    {
        transVehicleVel2WheelVel_four_mecanum(vehicleVel.x_move,vehicleVel.y_move,vehicleVel.z_rotate,wheelVel);
        break;
    }
    case E_VEHICLE_TYPE_ONE_STERRING_WHEEL:
    {
        if(E_VEHICLE_MODE_JOG==modeIn)
        {
                    return transVehicleVel2WheelVel_oneSteering(vehicleVel.x_move,vehicleVel.y_move,
                                                       vehicleVel.z_rotate,steeringAngleOut,wheelVel);
        }
        else
        {
            // kp=0.5 0.0７m到达误差，　ｋｐ超过0.7容易震荡。
            return transVehicleVel2WheelVel_oneSteering_Feedforward(vehicleVel.x_move,vehicleVel.y_move,
                                               vehicleVel.z_rotate,steeringAngleOut,wheelVel);


            // 0.05m到达误差
//                    return transVehicleVel2WheelVel_oneSteering_Gemotry(vehicleVel.x_move,vehicleVel.y_move,
//                                                       vehicleVel.z_rotate,steeringAngleOut,wheelVel);

                // kp=1 0.038m到达误差，kp=2时　左右摇摆震荡。ｋp=1.3 开始阶段偏摆４５ｄｅｇｒｅｅ
//                return transVehicleVel2WheelVel_oneSteering_RotationControlOnly(vehicleVel.x_move,vehicleVel.y_move,
//                                                   vehicleVel.z_rotate,steeringAngleOut,wheelVel);


            //        return transVehicleVel2WheelVel_oneSteering(vehicleVel.x_move,vehicleVel.y_move,
            //                                           vehicleVel.z_rotate,steeringAngleOut,wheelVel);
        }


        break;
    }
        default:
        {
        return -1;
        }
    }
    return 1;
}

int VehicleKinematics::transVehicleVel2WheelVel_two_differential(double linear_x, double angular_z,
                                                                 QVector<double>& wheelVel)
{
//    angular_z=-angular_z;//agv车头调换旋转反向。

    //convert agv m/s to motors m/s
    double linear_vel_left_in_m_s = linear_x -(angular_z*base_width_in_m)/2;
    double linear_vel_right_in_m_s = linear_x +(angular_z*base_width_in_m)/2;

    //convert motors m/s to degree/s
    wheelVel.resize(2);
    wheelVel[0]= linear_vel_left_in_m_s*360/ circumference_in_m;
    wheelVel[1] = linear_vel_right_in_m_s*360 / circumference_in_m;

    return 1;
}

int VehicleKinematics::transWheelVel2VehicleVel_four_mecanum(QVector<double> jointVel, VehicleVelocity &velOut)
{
    if(jointVel.size()<4)
    {
        return -1;
    }
    //degree/s to m/s
    for(int i=0;i<4;i++)
    {
        jointVel[i]=(jointVel[i]*M_PI/180.0)*(wheel_diameter_in_m/2.0);
    }


    VehicleVelocity tmpVel;
    tmpVel.y_move=(jointVel[0]+jointVel[1]+jointVel[2]+jointVel[3])/4.0;
    tmpVel.z_rotate=(jointVel[0]+jointVel[3]-2.0*tmpVel.y_move)
            /(base_width_in_m+base_heighth_in_m);
    tmpVel.x_move=(jointVel[1]+jointVel[3]-2.0*tmpVel.y_move)/2.0;

    //修改坐标系方向
    velOut.x_move=tmpVel.y_move;
    velOut.y_move=-tmpVel.x_move;
    velOut.z_rotate=tmpVel.z_rotate;


    return 1;

}

int VehicleKinematics::transWheelVel2VehicleVel_oneSteering(double wheelVel, double steeringAngle,
                                                            VehicleVelocity &velOut)
{
    double tmpSteeringRad=M_PI*steeringAngle/180.0;
    //degree/s
    double average_x = wheelVel*cos(tmpSteeringRad);
    //convert revolutions per second to m/s
    velOut.x_move = ((average_x/360) * (wheel_diameter_in_m * M_PI)); // m/s
    double average_a = wheelVel*sin(tmpSteeringRad);;
    //convert revolutions per second to rad/s
    velOut.z_rotate =  ((average_a/360) * (wheel_diameter_in_m * M_PI)) / (base_heighth_in_m);//rad/s
    velOut.y_move = 0.0;

//    qDebug()<<"velOut.x_move"<<velOut.x_move<<"average_x"<<average_x<<"wheelVel"<<wheelVel<<"cos(tmpSteeringRad)"<<cos(tmpSteeringRad);
//    velOut.z_rotate=-velOut.z_rotate;//agv车头换方向调换旋转反向。华磊

    return 1;
}

int VehicleKinematics::transVehicleVel2WheelVel_four_mecanum(double linear_x, double linear_y, double angular_z,
                                                             QVector<double> &wheelVel)
{
        wheelVel.resize(4);
        // temp
        const double temp1 = angular_z * ( base_width_in_m + base_heighth_in_m ) / 2.0;
        double wheelRadius=wheel_diameter_in_m/2.0;


        // wheel 1:
        wheelVel[0] = ( linear_x + linear_y + temp1 ) / wheelRadius;

        // wheel 2:
        wheelVel[1] = ( linear_x - linear_y - temp1 ) / wheelRadius;

        // wheel 3:
        wheelVel[2] = ( linear_x + linear_y - temp1 ) / wheelRadius;

        // wheel 4:
        wheelVel[3] = ( linear_x - linear_y + temp1 ) / wheelRadius;

        // rad/s to degree/s
        for(int i=0;i<4;i++)
        {
            wheelVel[i]=180.0*wheelVel[i]/M_PI;
        }
        return 1;

}
//手动使用
#define D_REASONBABLE_VEL_FOR_TAN2 0.0001
int VehicleKinematics::transVehicleVel2WheelVel_oneSteering(double linear_x, double linear_y, double angular_z,
                                             QVector<double>  &steeringAngleOut, QVector<double> &wheelVel)
{


//    static double lastSteeringAngle=0;//记录上次的值，
    double tmpAngle;
    //限制转向角度－９０到正９０

    // 线速度和角速度都非常小时，需要做特殊限制处理。
    if(fabs(angular_z*base_heighth_in_m)>D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
    {
        if(steeringPositionFeedback[0]<0)
        {
            tmpAngle=-M_PI_2;
        }
        else
        {
            tmpAngle=M_PI_2;
        }

    }
    else if(fabs(angular_z*base_heighth_in_m)<D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
    {
        //直线运动启动和停止时，线速度小，角速度大，导致转向角度有１ｓ　４５度左右的抖动。
        //线速度小于限定值时，转向角度不调整。
        wheelVel.resize(1);
        wheelVel[0]=0;
        steeringAngleOut.clear();
        return 0;//角速度和线速度都很小的时候，这时候是不确定的状态。
    }
    else
    {

        tmpAngle=atan2(angular_z*base_heighth_in_m,linear_x);
        //当前进时
        if(linear_x>=0)
        {

        }
        else//后退时,等效到前面象限控制
        {
            tmpAngle+=M_PI;
            tmpAngle=transformToPi(tmpAngle);
//            linear_x=-linear_x;

        }

    }



     wheelVel.resize(1);

     if(fabs(tmpAngle)<M_PI_4)//解决除０问题
     {
        //convert agv m/s to motors m/s
        double linear_vel_in_m_s = linear_x/cos(tmpAngle);

        //convert motors m/s to degree/s

        wheelVel[0]= linear_vel_in_m_s*360/ circumference_in_m;
     }
     else
     {
         //convert agv m/s to motors m/s
         double linear_vel_in_m_s = angular_z*base_heighth_in_m/sin(tmpAngle);

         //convert motors m/s to degree/s

         wheelVel[0]= linear_vel_in_m_s*360/ circumference_in_m;
     }


        steeringAngleOut.resize(1);
        steeringAngleOut[0]=180.0*tmpAngle/M_PI;

//        qDebug()<<"VehicleKinematics:: steeringAngleOut"<<steeringAngleOut[0]<<"steeringPositionFeedback[0]"
//               <<steeringPositionFeedback[0];



        return 1;
}
#define D_MAX_STEERING_ANGLE 65
#define D_MAX_ROTATION_ADJUST_ANGLE 6
#define D_MIN_VEL_FOR_STEERING 0.05
//steeringKp notused
int VehicleKinematics::transVehicleVel2WheelVel_oneSteering_RotationControlOnly(double linear_x,
             double linear_y, double angular_z, QVector<double> &steeringAngleOut, QVector<double> &wheelVel)
{
    double tmpAngle;
    //限制转向角度－９０到正９０

    static int isOnlyRotationCount=0;
    if(fabs(angular_z*base_heighth_in_m)>D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
    {
        isOnlyRotationCount++;
        //原地旋转.保证手动操作可以使下面的生效。
        if(isOnlyRotationCount>2)
        {
            int tmpKeyRotate=transVehicleVel2WheelVel_oneSteering(linear_x,linear_y,  angular_z,
                                                                  steeringAngleOut, wheelVel);
            if(1==tmpKeyRotate)
            {
                if(wheelVel[0]>wheelMaxSpeed)
                {

                    qDebug()<<"VehicleKinematics原地旋转 车轮速度"<<wheelVel[0]
                           <<"超正限制，强制降低到"<<wheelMaxSpeed;
                             wheelVel[0]=wheelMaxSpeed;
                }
                else if(wheelVel[0]<-wheelMaxSpeed)
                {
                    qDebug()<<"VehicleKinematics原地旋转 车轮速度"<<wheelVel[0]
                           <<"超负限制，强制降低到"<<-wheelMaxSpeed;
                    wheelVel[0]=-wheelMaxSpeed;
                }
            }
            return tmpKeyRotate;
        }


    }
    else if(fabs(angular_z*base_heighth_in_m)<D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
    {
        isOnlyRotationCount=0;
        //直线运动启动和停止时，线速度小，角速度大，导致转向角度有１ｓ　４５度左右的抖动。
        //线速度小于限定值时，转向角度不调整。
        wheelVel.resize(1);
        wheelVel[0]=0;
        steeringAngleOut.clear();
        return 0;//角速度和线速度都很小的时候，这时候是不确定的状态。
    }
    else
    {
        isOnlyRotationCount=0;
        double tmpEqualVel=fabs(linear_x);
        if(tmpEqualVel<0.01)//linear_x很小时，会导致转角异常大
        {
            tmpEqualVel=D_MIN_VEL_FOR_STEERING*5;
        }
        else if(tmpEqualVel<0.02)
        {
            tmpEqualVel=D_MIN_VEL_FOR_STEERING*4;
        }
        else if(tmpEqualVel<0.03)
        {
            tmpEqualVel=D_MIN_VEL_FOR_STEERING*3;
        }
        else if(tmpEqualVel<0.04)
        {
            tmpEqualVel=D_MIN_VEL_FOR_STEERING*2;
        }
        else if(tmpEqualVel<D_MIN_VEL_FOR_STEERING)
        {
            tmpEqualVel=D_MIN_VEL_FOR_STEERING;
        }
        else
        {
            tmpEqualVel=D_MIN_VEL_FOR_STEERING;//不和速度关联，因为virtualLine计算已经关联过了。
        }

        //steeringKp=50,跟随误差较好小于１０mm，抖动约２０ｄｅｇｒｅｅ
        //steeringKp=5,跟随误差较好小于5０mm，抖动约5ｄｅｇｒｅｅ
        tmpAngle=angular_z*steeringKp/tmpEqualVel;
        double maxSteer=M_PI*D_MAX_ROTATION_ADJUST_ANGLE/180.0;
        if(tmpAngle>maxSteer)
        {
            tmpAngle=maxSteer;
        }
        else if(tmpAngle<-maxSteer)
        {
            tmpAngle=-maxSteer;
        }

        if(linear_x<0)
        {
            tmpAngle=-tmpAngle;//倒退时需要反方向
        }


    }


     wheelVel.resize(1);

//         //忽略转角，直接等效。
         double linear_vel_in_m_s = linear_x;
         wheelVel[0]= linear_vel_in_m_s*360/ circumference_in_m;


     steeringAngleOut.resize(1);
     steeringAngleOut[0]=180.0*tmpAngle/M_PI;

     if(1==debugFlag)
     {
         qDebug()<<"1rotationControlOnly steering"<<steeringAngleOut[0]
                   <<"wheelVel"<<wheelVel[0];
     }

     if(wheelVel[0]>wheelMaxSpeed)
     {

         qDebug()<<"VehicleKinematics 车轮速度"<<wheelVel[0]
                <<"超正限制，强制降低到"<<wheelMaxSpeed;
                  wheelVel[0]=wheelMaxSpeed;
     }
     else if(wheelVel[0]<-wheelMaxSpeed)
     {
         qDebug()<<"VehicleKinematics 车轮速度"<<wheelVel[0]
                <<"超负限制，强制降低到"<<-wheelMaxSpeed;
         wheelVel[0]=-wheelMaxSpeed;
     }


       if(fabs(linear_x)<0.001)//转角过渡完了，开始车体旋转的几个周期需要用到这个限制
       {

           steeringAngleOut.clear();
           return 0;

       }
       else
       {
   //        qDebug()<<"VehicleKinematics:: steeringAngleOut"<<steeringAngleOut[0]<<"steeringPositionFeedback[0]"
   //               <<steeringPositionFeedback[0];
       }



       return 1;
}

//steeringKp used
int VehicleKinematics::transVehicleVel2WheelVel_oneSteering_RotationControl(bool isPositiveRun,double feedAngleIn,
                                                                            double linear_x, double linear_y,
                       double angular_z, QVector<double> &steeringAngleOut, QVector<double> &wheelVel)
{

        double tmpFeedAngle=M_PI*feedAngleIn/180.0;
    //    static double lastSteeringAngle=0;//记录上次的值，
        double tmpAngle;
        //限制转向角度－９０到正９０

        static int isOnlyRotationCount=0;
        if(fabs(angular_z*base_heighth_in_m)>D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
        {
            isOnlyRotationCount++;
            //原地旋转.保证手动操作可以使下面的生效。
            if(isOnlyRotationCount>2)
            {
                return transVehicleVel2WheelVel_oneSteering(linear_x,linear_y,  angular_z,
                                                            steeringAngleOut, wheelVel);
            }


        }
        else if(fabs(angular_z*base_heighth_in_m)<D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
        {
            isOnlyRotationCount=0;
            //直线运动启动和停止时，线速度小，角速度大，导致转向角度有１ｓ　４５度左右的抖动。
            //线速度小于限定值时，转向角度不调整。
            wheelVel.resize(1);
            wheelVel[0]=0;
            steeringAngleOut.clear();
            return 0;//角速度和线速度都很小的时候，这时候是不确定的状态。
        }
        else
        {
            isOnlyRotationCount=0;
            double tmpEqualVel=fabs(linear_x);

//            //0.5m/s以下没有表现摆动，超过就摆动了.所以必须和速度关联。
//            if(tmpEqualVel<D_MIN_VEL_FOR_STEERING)
//            {
//                tmpEqualVel=D_MIN_VEL_FOR_STEERING;
//            }
//            else if(tmpEqualVel<0.3)
//            {
//                tmpEqualVel=tmpEqualVel;
//            }
//            else if(tmpEqualVel<0.5)
//            {
//                tmpEqualVel=tmpEqualVel*2;
//            }
//            else if(tmpEqualVel<0.7)
//            {
//                tmpEqualVel=tmpEqualVel*2.6;//1.6还是有肉眼可见摆动,3.6时纠偏不足，导致低速时摆动纠偏。
//            }
//            else if(tmpEqualVel<1.0)
//            {
//                tmpEqualVel=tmpEqualVel*3.9;
//            }
//            else
//            {
//                tmpEqualVel=tmpEqualVel*4.2;
//            }
            tmpEqualVel=D_MIN_VEL_FOR_STEERING;//和速度是否关联，效果差不多．
            //steeringKp=50,跟随误差较好小于１０mm，抖动约２０ｄｅｇｒｅｅ
            //steeringKp=5,跟随误差较好小于5０mm，抖动约5ｄｅｇｒｅｅ
            tmpAngle=angular_z*steeringKp/tmpEqualVel;
            //拐弯时，需要加大调节量限制．
            double tmpNewAdjustMax=maxSteerAdjust+fabs(tmpFeedAngle)/0.5;
            if(tmpAngle>tmpNewAdjustMax)
            {
                tmpAngle=tmpNewAdjustMax;
                qDebug()<<"tmpAngle>maxSteerAdjust"<<tmpNewAdjustMax;
            }
            else if(tmpAngle<-tmpNewAdjustMax)
            {
                tmpAngle=-tmpNewAdjustMax;
                qDebug()<<"tmpAngle<-maxSteerAdjust"<<-tmpNewAdjustMax;
            }

//            if(linear_x<0)
//            {
//                tmpAngle=-tmpAngle;//倒退时需要反方向
//            }

            if(isPositiveRun)
            {

            }
            else
            {

                tmpAngle=-tmpAngle;//倒退时需要反方向
                qDebug()<<"倒退时需要反方向"<<tmpAngle;
            }


        }


         wheelVel.resize(1);

//         //忽略转角，直接等效。
//         double linear_vel_in_m_s = linear_x;
//         wheelVel[0]= linear_vel_in_m_s*360/ circumference_in_m;

         if(fabs(tmpFeedAngle)<M_PI_4)//解决除０问题
         {
            //convert agv m/s to motors m/s
            double linear_vel_in_m_s = linear_x/cos(tmpFeedAngle);

            //convert motors m/s to degree/s

            wheelVel[0]= linear_vel_in_m_s*360/ circumference_in_m;
         }
         else
         {
             //convert agv m/s to motors m/s
             double linear_vel_in_m_s = angular_z*base_heighth_in_m/sin(tmpFeedAngle);

             //convert motors m/s to degree/s

             wheelVel[0]= linear_vel_in_m_s*360/ circumference_in_m;
         }



         steeringAngleOut.resize(1);
         steeringAngleOut[0]=180.0*tmpAngle/M_PI;

//         qDebug()<<"1rotationControl steering"<<steeringAngleOut[0]
//                   <<"wheelVel"<<wheelVel[0];

//           if(fabs(linear_x)<0.001)//转角过渡完了，开始车体旋转的几个周期需要用到这个限制
//           {

//               steeringAngleOut.clear();
//               return 0;

//           }
//           else
//           {
//       //        qDebug()<<"VehicleKinematics:: steeringAngleOut"<<steeringAngleOut[0]<<"steeringPositionFeedback[0]"
//       //               <<steeringPositionFeedback[0];
//           }



           return 1;
}

int VehicleKinematics::transVehicleVel2WheelVel_oneSteering_Gemotry(double linear_x,
                 double linear_y, double angular_z, QVector<double> &steeringAngleOut, QVector<double> &wheelVel)
{
    //    static double lastSteeringAngle=0;//记录上次的值，
        double tmpAngle;
        //限制转向角度－９０到正９０

        static int isOnlyRotationCount=0;
        if(fabs(angular_z*base_heighth_in_m)>D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
        {
            isOnlyRotationCount++;
            //原地旋转.保证手动操作可以使下面的生效。
            if(isOnlyRotationCount>5)
            {
                return transVehicleVel2WheelVel_oneSteering(linear_x,linear_y,  angular_z,
                                                            steeringAngleOut, wheelVel);
            }


        }
        else if(fabs(angular_z*base_heighth_in_m)<D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
        {
            isOnlyRotationCount=0;
            //直线运动启动和停止时，线速度小，角速度大，导致转向角度有１ｓ　４５度左右的抖动。
            //线速度小于限定值时，转向角度不调整。
            wheelVel.resize(1);
            wheelVel[0]=0;
            steeringAngleOut.clear();
            return 0;//角速度和线速度都很小的时候，这时候是不确定的状态。
        }
        else
        {
            isOnlyRotationCount=0;

            transVehicleVel2WheelVel_oneSteering(linear_x,linear_y,  angular_z,
                                                                        steeringAngleOut, wheelVel);

        }




           if(fabs(linear_x)<0.001)//转角过渡完了，开始车体旋转的几个周期需要用到这个限制
           {

               steeringAngleOut.clear();
       //        wheelVel.clear();

       //        steeringAngleOut.resize(1);
       //        steeringAngleOut[0]=steeringPositionFeedback[0]
       //        steeringAngleOut[0]=180.0*tmpAngle/M_PI;
       //        qDebug()<<"VehicleKinematics not steering tmpAngle-steeringPositionFeedback[0]"
       //               <<tmpAngle-steeringPositionFeedback[0];
           }

           if(steeringAngleOut.size()>0)
           {
               double maxSteer=D_MAX_STEERING_ANGLE;
               if(steeringAngleOut[0]>maxSteer)
               {
                   steeringAngleOut[0]=maxSteer;
               }
               else if(steeringAngleOut[0]<-maxSteer)
               {
                   steeringAngleOut[0]=-maxSteer;
               }
           }



           return 1;
}

//跟线使用
int VehicleKinematics::transVehicleVel2WheelVel_oneSteering_Feedforward(double linear_x, double linear_y,
                          double angular_z, QVector<double> &steeringAngleOut, QVector<double> &wheelVel)
{
    //    static double lastSteeringAngle=0;//记录上次的值，
        double tmpAngle;
        //限制转向角度－９０到正９０

        static int isOnlyRotationCount=0;
        if(fabs(angular_z*base_heighth_in_m)>D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
        {
            isOnlyRotationCount++;
            //原地旋转.保证手动操作可以使下面的生效。
            if(isOnlyRotationCount>3)
            {
                int tmpKeyRotate= transVehicleVel2WheelVel_oneSteering(linear_x,linear_y,  angular_z,
                                                            steeringAngleOut, wheelVel);

                if(1==tmpKeyRotate)
                {
                    if(wheelVel[0]>wheelMaxSpeed)
                    {

                        qDebug()<<"VehicleKinematics原地旋转 车轮速度"<<wheelVel[0]
                               <<"超正限制，强制降低到"<<wheelMaxSpeed;
                                 wheelVel[0]=wheelMaxSpeed;
                    }
                    else if(wheelVel[0]<-wheelMaxSpeed)
                    {
                        qDebug()<<"VehicleKinematics原地旋转 车轮速度"<<wheelVel[0]
                               <<"超负限制，强制降低到"<<-wheelMaxSpeed;
                        wheelVel[0]=-wheelMaxSpeed;
                    }
                }
                return tmpKeyRotate;
            }


        }
        else if(fabs(angular_z*base_heighth_in_m)<D_REASONBABLE_VEL_FOR_TAN2 && fabs(linear_x)<D_REASONBABLE_VEL_FOR_TAN2)
        {
            isOnlyRotationCount=0;
            //直线运动启动和停止时，线速度小，角速度大，导致转向角度有１ｓ　４５度左右的抖动。
            //线速度小于限定值时，转向角度不调整。
            wheelVel.resize(1);
            wheelVel[0]=0;
            steeringAngleOut.clear();
            return 0;//角速度和线速度都很小的时候，这时候是不确定的状态。
        }
        else
        {
            isOnlyRotationCount=0;
            //前馈使用几何解法
            QVector<double> steeringAngle1, wheelVel1;
            int tmpKey1=transVehicleVel2WheelVel_oneSteering(feedforward_x,feedforward_y,  feedforward_r,
                                                                        steeringAngle1, wheelVel1);
            if(1!=tmpKey1)
            {
                 steeringAngleOut.clear();
                 wheelVel.clear();
                 return 0;
            }
            //剩余偏差使用转角控制方法
            QVector<double> steeringAngle2, wheelVel2;
            bool tmpIsPositive;
            if(feedforward_x>0)
            {
                tmpIsPositive=true;
            }
            else
            {
                tmpIsPositive=false;
            }
            ////steeringKp
            int tmpKey2=transVehicleVel2WheelVel_oneSteering_RotationControl(tmpIsPositive,steeringAngle1[0],
                                                              linear_x-feedforward_x,
                              linear_y-feedforward_y,  angular_z-feedforward_r,
                               steeringAngle2, wheelVel2);
            if(1==tmpKey1 &&  1==tmpKey2)
            {
                wheelVel.resize(1);
                wheelVel[0]=wheelVel1[0]+wheelVel2[0];
                steeringAngleOut.resize(1);
                steeringAngleOut[0]=steeringAngle1[0]+steeringAngle2[0];
                if(0)
                {
                    qDebug()<<"1aaa steering"<<steeringAngle1[0]<<steeringAngle2[0]
                           <<"wheelVel"<<wheelVel1[0]<<wheelVel2[0]<<"angular_z-feedforward_r"<<angular_z-feedforward_r
                             <<"steeringAngleOut"<<steeringAngleOut<<"steeringAngle1"<<steeringAngle1[0]
                            <<"steeringAngle2"<<steeringAngle2[0]<<"degree";
                }

            }
            else if(1==tmpKey1 &&  1!=tmpKey2)
            {
                wheelVel=wheelVel1;
                steeringAngleOut=steeringAngle1;
                if(0)
                {
                    qDebug()<<"2aaa steering"<<steeringAngle1[0]
                              <<"wheelVel"<<wheelVel1[0];
                }

            }
            else
            {
                 steeringAngleOut.clear();
                 wheelVel.clear();
                 return 0;
            }

        }



           if(fabs(linear_x)<0.001)//转角过渡完了，开始车体旋转的几个周期需要用到这个限制
           {
               steeringAngleOut.clear();
           }

           if(steeringAngleOut.size()>0)
           {
               double maxSteer=D_MAX_STEERING_ANGLE;
               if(steeringAngleOut[0]>maxSteer)
               {
                   steeringAngleOut[0]=maxSteer;
               }
               else if(steeringAngleOut[0]<-maxSteer)
               {
                   steeringAngleOut[0]=-maxSteer;
               }
           }

           if(wheelVel.size()>0)
           {
               if(wheelVel[0]>wheelMaxSpeed)
               {

                   qDebug()<<"VehicleKinematics 车轮速度"<<wheelVel[0]
                          <<"超正限制，强制降低到"<<wheelMaxSpeed;
                            wheelVel[0]=wheelMaxSpeed;
               }
               else if(wheelVel[0]<-wheelMaxSpeed)
               {
                   qDebug()<<"VehicleKinematics 车轮速度"<<wheelVel[0]
                          <<"超负限制，强制降低到"<<-wheelMaxSpeed;
                   wheelVel[0]=-wheelMaxSpeed;
               }
           }




           return 1;
}

double VehicleKinematics::transformToPi(double angleIn)
{
    //    int turns=angleIn/M_PI/2;//可能bug 参考getTurnsFromRad
        int turns;

        if(0<angleIn)
        {
            turns=(angleIn+M_PI) /(2*M_PI);
        }
        else
        {
            turns=(angleIn-M_PI) /(2*M_PI);
        }

        //    angleIn=angleIn-turns*M_PI;//错误
            angleIn=angleIn-turns*M_PI*2;

        if(angleIn>=-M_PI && angleIn<=M_PI)
        {
            return angleIn;
        }
        else if(angleIn<-M_PI)
        {
            return angleIn+2*M_PI;
        }
        else if(angleIn>M_PI)
        {
            return angleIn-2*M_PI;
        }

        return angleIn;
}

int VehicleKinematics::transWheelVel2VehicleVel_two_differential(double left_wheel_vel,
                                            double right__wheel_vel, VehicleVelocity& velOut)
{

    //degree/s
    double average_x = (left_wheel_vel + right__wheel_vel) / 2;
    //convert revolutions per second to m/s
    velOut.x_move = ((average_x/360) * (wheel_diameter_in_m * M_PI)); // m/s
    double average_a = (right__wheel_vel-left_wheel_vel) / 2;
    //convert revolutions per second to rad/s
    velOut.z_rotate =  ((average_a/360) * (wheel_diameter_in_m * M_PI)) / (base_width_in_m / 2);//rad/s
    velOut.y_move = 0.0;

//    velOut.z_rotate=-velOut.z_rotate;//agv车头换方向调换旋转反向。华磊

    return 1;
}
